namespace Eigen {

namespace internal {

template<typename Scalar>
void
lmpar(Matrix<Scalar, Dynamic, Dynamic>& r,
	  const VectorXi& ipvt,
	  const Matrix<Scalar, Dynamic, 1>& diag,
	  const Matrix<Scalar, Dynamic, 1>& qtb,
	  Scalar delta,
	  Scalar& par,
	  Matrix<Scalar, Dynamic, 1>& x)
{
	using std::abs;
	using std::sqrt;
	typedef DenseIndex Index;

	/* Local variables */
	Index i, j, l;
	Scalar fp;
	Scalar parc, parl;
	Index iter;
	Scalar temp, paru;
	Scalar gnorm;
	Scalar dxnorm;

	/* Function Body */
	const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
	const Index n = r.cols();
	eigen_assert(n == diag.size());
	eigen_assert(n == qtb.size());
	eigen_assert(n == x.size());

	Matrix<Scalar, Dynamic, 1> wa1, wa2;

	/* compute and store in x the gauss-newton direction. if the */
	/* jacobian is rank-deficient, obtain a least squares solution. */
	Index nsing = n - 1;
	wa1 = qtb;
	for (j = 0; j < n; ++j) {
		if (r(j, j) == 0. && nsing == n - 1)
			nsing = j - 1;
		if (nsing < n - 1)
			wa1[j] = 0.;
	}
	for (j = nsing; j >= 0; --j) {
		wa1[j] /= r(j, j);
		temp = wa1[j];
		for (i = 0; i < j; ++i)
			wa1[i] -= r(i, j) * temp;
	}

	for (j = 0; j < n; ++j)
		x[ipvt[j]] = wa1[j];

	/* initialize the iteration counter. */
	/* evaluate the function at the origin, and test */
	/* for acceptance of the gauss-newton direction. */
	iter = 0;
	wa2 = diag.cwiseProduct(x);
	dxnorm = wa2.blueNorm();
	fp = dxnorm - delta;
	if (fp <= Scalar(0.1) * delta) {
		par = 0;
		return;
	}

	/* if the jacobian is not rank deficient, the newton */
	/* step provides a lower bound, parl, for the zero of */
	/* the function. otherwise set this bound to zero. */
	parl = 0.;
	if (nsing >= n - 1) {
		for (j = 0; j < n; ++j) {
			l = ipvt[j];
			wa1[j] = diag[l] * (wa2[l] / dxnorm);
		}
		// it's actually a triangularView.solveInplace(), though in a weird
		// way:
		for (j = 0; j < n; ++j) {
			Scalar sum = 0.;
			for (i = 0; i < j; ++i)
				sum += r(i, j) * wa1[i];
			wa1[j] = (wa1[j] - sum) / r(j, j);
		}
		temp = wa1.blueNorm();
		parl = fp / delta / temp / temp;
	}

	/* calculate an upper bound, paru, for the zero of the function. */
	for (j = 0; j < n; ++j)
		wa1[j] = r.col(j).head(j + 1).dot(qtb.head(j + 1)) / diag[ipvt[j]];

	gnorm = wa1.stableNorm();
	paru = gnorm / delta;
	if (paru == 0.)
		paru = dwarf / (std::min)(delta, Scalar(0.1));

	/* if the input par lies outside of the interval (parl,paru), */
	/* set par to the closer endpoint. */
	par = (std::max)(par, parl);
	par = (std::min)(par, paru);
	if (par == 0.)
		par = gnorm / dxnorm;

	/* beginning of an iteration. */
	while (true) {
		++iter;

		/* evaluate the function at the current value of par. */
		if (par == 0.)
			par = (std::max)(dwarf, Scalar(.001) * paru); /* Computing MAX */
		wa1 = sqrt(par) * diag;

		Matrix<Scalar, Dynamic, 1> sdiag(n);
		qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);

		wa2 = diag.cwiseProduct(x);
		dxnorm = wa2.blueNorm();
		temp = fp;
		fp = dxnorm - delta;

		/* if the function is small enough, accept the current value */
		/* of par. also test for the exceptional cases where parl */
		/* is zero or the number of iterations has reached 10. */
		if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
			break;

		/* compute the newton correction. */
		for (j = 0; j < n; ++j) {
			l = ipvt[j];
			wa1[j] = diag[l] * (wa2[l] / dxnorm);
		}
		for (j = 0; j < n; ++j) {
			wa1[j] /= sdiag[j];
			temp = wa1[j];
			for (i = j + 1; i < n; ++i)
				wa1[i] -= r(i, j) * temp;
		}
		temp = wa1.blueNorm();
		parc = fp / delta / temp / temp;

		/* depending on the sign of the function, update parl or paru. */
		if (fp > 0.)
			parl = (std::max)(parl, par);
		if (fp < 0.)
			paru = (std::min)(paru, par);

		/* compute an improved estimate for par. */
		/* Computing MAX */
		par = (std::max)(parl, par + parc);

		/* end of an iteration. */
	}

	/* termination. */
	if (iter == 0)
		par = 0.;
	return;
}

template<typename Scalar>
void
lmpar2(const ColPivHouseholderQR<Matrix<Scalar, Dynamic, Dynamic>>& qr,
	   const Matrix<Scalar, Dynamic, 1>& diag,
	   const Matrix<Scalar, Dynamic, 1>& qtb,
	   Scalar delta,
	   Scalar& par,
	   Matrix<Scalar, Dynamic, 1>& x)

{
	using std::abs;
	using std::sqrt;
	typedef DenseIndex Index;

	/* Local variables */
	Index j;
	Scalar fp;
	Scalar parc, parl;
	Index iter;
	Scalar temp, paru;
	Scalar gnorm;
	Scalar dxnorm;

	/* Function Body */
	const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
	const Index n = qr.matrixQR().cols();
	eigen_assert(n == diag.size());
	eigen_assert(n == qtb.size());

	Matrix<Scalar, Dynamic, 1> wa1, wa2;

	/* compute and store in x the gauss-newton direction. if the */
	/* jacobian is rank-deficient, obtain a least squares solution. */

	//    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
	const Index rank = qr.rank(); // use a threshold
	wa1 = qtb;
	wa1.tail(n - rank).setZero();
	qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));

	x = qr.colsPermutation() * wa1;

	/* initialize the iteration counter. */
	/* evaluate the function at the origin, and test */
	/* for acceptance of the gauss-newton direction. */
	iter = 0;
	wa2 = diag.cwiseProduct(x);
	dxnorm = wa2.blueNorm();
	fp = dxnorm - delta;
	if (fp <= Scalar(0.1) * delta) {
		par = 0;
		return;
	}

	/* if the jacobian is not rank deficient, the newton */
	/* step provides a lower bound, parl, for the zero of */
	/* the function. otherwise set this bound to zero. */
	parl = 0.;
	if (rank == n) {
		wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2) / dxnorm;
		qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
		temp = wa1.blueNorm();
		parl = fp / delta / temp / temp;
	}

	/* calculate an upper bound, paru, for the zero of the function. */
	for (j = 0; j < n; ++j)
		wa1[j] = qr.matrixQR().col(j).head(j + 1).dot(qtb.head(j + 1)) / diag[qr.colsPermutation().indices()(j)];

	gnorm = wa1.stableNorm();
	paru = gnorm / delta;
	if (paru == 0.)
		paru = dwarf / (std::min)(delta, Scalar(0.1));

	/* if the input par lies outside of the interval (parl,paru), */
	/* set par to the closer endpoint. */
	par = (std::max)(par, parl);
	par = (std::min)(par, paru);
	if (par == 0.)
		par = gnorm / dxnorm;

	/* beginning of an iteration. */
	Matrix<Scalar, Dynamic, Dynamic> s = qr.matrixQR();
	while (true) {
		++iter;

		/* evaluate the function at the current value of par. */
		if (par == 0.)
			par = (std::max)(dwarf, Scalar(.001) * paru); /* Computing MAX */
		wa1 = sqrt(par) * diag;

		Matrix<Scalar, Dynamic, 1> sdiag(n);
		qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);

		wa2 = diag.cwiseProduct(x);
		dxnorm = wa2.blueNorm();
		temp = fp;
		fp = dxnorm - delta;

		/* if the function is small enough, accept the current value */
		/* of par. also test for the exceptional cases where parl */
		/* is zero or the number of iterations has reached 10. */
		if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
			break;

		/* compute the newton correction. */
		wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2 / dxnorm);
		// we could almost use this here, but the diagonal is outside qr, in sdiag[]
		// qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
		for (j = 0; j < n; ++j) {
			wa1[j] /= sdiag[j];
			temp = wa1[j];
			for (Index i = j + 1; i < n; ++i)
				wa1[i] -= s(i, j) * temp;
		}
		temp = wa1.blueNorm();
		parc = fp / delta / temp / temp;

		/* depending on the sign of the function, update parl or paru. */
		if (fp > 0.)
			parl = (std::max)(parl, par);
		if (fp < 0.)
			paru = (std::min)(paru, par);

		/* compute an improved estimate for par. */
		par = (std::max)(parl, par + parc);
	}
	if (iter == 0)
		par = 0.;
	return;
}

} // end namespace internal

} // end namespace Eigen
